irPort = 1
cpPort = 2
motorPortL = 1
motorPortR = 3

nxt.I2Cversion    = string.char( 0x10, 0x00 )
nxt.I2Cproduct    = string.char( 0x10, 0x08 )
nxt.I2Ctype       = string.char( 0x10, 0x10 )
nxt.I2Ccontinuous = string.char( 0x10, 0x41, 0x02 )
nxt.I2Cdata       = string.char( 0x10, 0x42 )


function waitI2C( port )
  while( 0 ~= nxt.I2CGetStatus( port ) ) do
  end
end

function setupI2C(port)
  nxt.InputSetType(port,2)
  nxt.InputSetDir(port,1,1)
  nxt.InputSetState(port,1,1)

  nxt.I2CInitPins(port)
end

function IRHeading(port)
	nxt.I2CSendData(port, string.char( 0x10, 0x42 ), 8)
	waitI2C(port)
	return string.byte(nxt.I2CRecvData(port))
end
function compHeading(port)
	nxt.I2CSendData(port,  string.char( 0x02, 0x42 ), 4)
	waitI2C(port)
	data = nxt.I2CRecvData(port, 4)
	b1, b2, b3, b4 = string.byte(data, 1, 8) 
	if b4 == 1 then
		truevalue = b3 + 255
	else
		truevalue = b3
	end
	return truevalue
end

function addDeg(a, b)
	c = a + b
	if c >= 360 then
		c = c - 360
	end
	if c < 0 then
		c = c + 360
	end
	return c
end
function degDiff(a, b)
	c = a - b
	if c < 0 then
		c = c  * -1
	end
	if c > 180 then
		c = 360 - c
	end
	return c
end

-- Driving functions
function steerForward()
	nxt.OutputSetSpeed(motorPortL, 32, -80)
	nxt.OutputSetSpeed(motorPortR, 32, -80)
end
function steerLeft()
	nxt.OutputSetSpeed(motorPortL, 32, 80)
	nxt.OutputSetSpeed(motorPortR, 32, -80)
end
function steerRight()
	nxt.OutputSetSpeed(motorPortL, 32, -80)
	nxt.OutputSetSpeed(motorPortR, 32, 80)
end
function allStop()
	nxt.OutputSetSpeed(motorPortL, 0, 0)
	nxt.OutputSetSpeed(motorPortR, 0, 0)
end

setupI2C(irPort)
setupI2C(cpPort)
goal = 100
speed = 60
while nxt.ButtonRead() == 0 do
	curHeading = addDeg(30, (IRHeading(irPort)+6)*30)
	if curHeading > 180 then
		curHeading = curHeading -360
	end
	cpHeading = compHeading(cpPort)
	ballHeading = addDeg(curHeading, cpHeading)
	forward = true
	if degDiff(ballHeading, goal) > 90 then
		forward = false
	end
	print("H: "..cpHeading.." IR_H: "..curHeading.." BL_H: "..ballHeading)
	nxt.DisplayClear()
	nxt.DisplayText("H: "..cpHeading, 0, 0)
	nxt.DisplayText("IR_H: "..curHeading, 0, 8)
	nxt.DisplayText("BL_H: "..ballHeading, 0, 16)
	if forward == true then
		nxt.DisplayText("ATK: YES", 0, 24) 
		if curHeading > 0 then
			nxt.OutputSetSpeed(motorPortL, 32, speed)
			nxt.OutputSetSpeed(motorPortR, 32, -speed)
		else
			nxt.OutputSetSpeed(motorPortL, 32, -speed)
			nxt.OutputSetSpeed(motorPortR, 32, speed)
		end
	else
		nxt.DisplayText("ATK: YES", 0, 24)
	end
	if curHeading == 0 then
		allStop()
	end
	print(speed)
	
	a = nxt.TimerRead()
	while nxt.TimerRead() < (a + 50) do
	end 
end
allStop()